#include "communication.h"
#include "uart.h"
#include "stdio.h"

uint8_t receiveBuff[FRAME_LEN] = {0};

typedef union {
    int16_t d;
    uint8_t data[2];
} int16_u;

static int16_u leftVelSet, rightVelSet, angleNow;
static int16_u leftVelNow, rightVelNow;

uint8_t getCrc8(uint8_t *ptr, uint16_t len) {
    uint8_t crc = 0;
    while (len--) {
        crc ^= *ptr++;
        for (uint8_t i = 0; i < 8; i++) {
            if (crc & 0x01)
                crc = (crc >> 1) ^ 0x8C;
            else
                crc >>= 1;
        }
    }
    return crc;
}

void USART1_IRQHandler(void) {
    if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) {
        uint8_t data = USART_ReceiveData(USART1);
        usartReceiveOneData(&leftVelSet.d, &rightVelSet.d, (uint8_t*)&angleNow.d);
        (void)data; 
    }
}

int usartReceiveOneData(int *p_leftSpeedSet, int *p_rightSpeedSet, uint8_t *p_ctrlFlag) {
    static uint8_t state = 0;
    static uint8_t index = 0;
    static uint8_t length = 0;

    uint8_t data = USART_ReceiveData(USART1);

    switch (state) {
    case 0: 
        if (data == HEADER1) state = 1;
        break;

    case 1: 
        if (data == HEADER2) {
            state = 2;
            index = 0;
        } else state = 0;
        break;

    case 2: 
        length = data;
        if (length == PAYLOAD_LEN) {
            receiveBuff[0] = HEADER1;
            receiveBuff[1] = HEADER2;
            receiveBuff[2] = length;
            state = 3;
        } else {
            state = 0;
        }
        break;

    case 3: 
        receiveBuff[3 + index++] = data;
        if (index >= length) state = 4;
        break;

    case 4: 
        receiveBuff[3 + length] = data;
        if (getCrc8(receiveBuff, 3 + length) == data) {
            state = 5;
        } else {
            printf("CRC error!\n");
            state = 0;
        }
        break;

    case 5: 
        if (data == ENDER1) state = 6;
        else state = 0;
        break;

    case 6: 
        if (data == ENDER2) {
            
            leftVelSet.data[0]  = receiveBuff[3];
            leftVelSet.data[1]  = receiveBuff[4];
            rightVelSet.data[0] = receiveBuff[5];
            rightVelSet.data[1] = receiveBuff[6];
            *p_ctrlFlag = receiveBuff[7];

            *p_leftSpeedSet  = leftVelSet.d;
            *p_rightSpeedSet = rightVelSet.d;

        }
        state = 0;
        break;
    }
    return 0;
}

void USART_Send_String(uint8_t *p, uint16_t sendSize) {
    for (uint16_t i = 0; i < sendSize; i++) {
        while (!(USART1->SR & (1 << 7))); 
        USART1->DR = p[i];
    }
}

void usartSendData(int16_t leftVel, int16_t rightVel, int16_t angle, uint8_t ctrlFlag) {
    uint8_t buf[FRAME_LEN] = {0};

    leftVelNow.d  = leftVel;
    rightVelNow.d = rightVel;
    angleNow.d    = angle;

    buf[0] = HEADER1;
    buf[1] = HEADER2;
    buf[2] = PAYLOAD_LEN;

    buf[3] = leftVelNow.data[0];
    buf[4] = leftVelNow.data[1];
    buf[5] = rightVelNow.data[0];
    buf[6] = rightVelNow.data[1];
    buf[7] = angleNow.data[0];
    buf[8] = angleNow.data[1];
    buf[9] = ctrlFlag;

    buf[10] = getCrc8(buf, 3 + PAYLOAD_LEN);
    buf[11] = ENDER1;
    buf[12] = ENDER2;

    USART_Send_String(buf, FRAME_LEN);
}



